#include "Euler.hpp"


/// ------------ construct/destruct --------------
Cabs_integr::Cabs_integr(point* p, uint s, wire* w, uint ws):
	P(p), size(s), W(w), wsize(ws) {
}

CEuler::CEuler(point* p, uint s, wire* w, uint ws): t(0) {
    getsize() = s;
    getwsize() = ws;
    setptrs(p, w);
}

CRK4::CRK4(point* p, uint s, wire* w, uint ws):
    final_pos(NULL), final_vel(NULL), t(0){
    getsize() = s;
    getwsize() = ws;
    setptrs(p, w);
    final_pos = new vec[s];
	final_vel = new vec[s];
	store_pos = new vec[s];
	store_vel = new vec[s];
}

CRK4::~CRK4() {
	if(final_pos) {
	    delete[] final_pos;
	}
	if(final_vel) {
	    delete[] final_vel;
	}
	if(store_pos) {
	    delete[] store_pos;
	}
	if(store_vel) {
	   delete[] store_vel;
	}
}
/// ------------- end of constr/destr ------------

/// ------------- set-get thing ------------------
uint& Cabs_integr::getsize() {
    return size;
}

uint& Cabs_integr::getwsize() {
    return wsize;
}

wire& Cabs_integr::getwire(uint& nr) {
    return W[nr];
}

point& Cabs_integr::getpoint(uint& nr) {
    return P[nr];
}

void Cabs_integr::setptrs(point* p, wire* w) {
    P = p;
    W = w;
}
/// ----------- end of set-get thing -------------


/// --------------------- methods ----------------

// calcs force through wires and distributes it
void Cabs_integr::prepare_force() {
	float current_len, alpha;
	vec force;
	for(uint i = 0; i < wsize; i++) {
	    W[i].length();
		current_len = W[i].len.len() - W[i].rest;
		alpha = W[i].len.arct();
		force.y = fabs(current_len) * sin(alpha);
		force.x = fabs(current_len) * cos(alpha);
		force *= W[i].koof;
		force /= 2;
		if(current_len < 0.0) {
			W[i].ps->fo -= force;
			W[i].pe->fo += force;
		}
		else {
			W[i].ps->fo += force;
			W[i].pe->fo -= force;
		}
	}
}

// returns derivatives in dvel and dpos of point by argument
void Cabs_integr::eval_derr(float time, point& pp) {
	pp.dpos = pp.dvel;
	pp.dvel = pp.fo / pp.mass - pp.vel * DAMPING / pp.mass;
}

// main integrating method for all point using Euler
// uses step h, until time = endt
void CEuler::solve(float h, float endt) {
    uint size = getsize();
	for(; endt > 0; endt -= h) {
		for(uint i = 0; i < size; i++) {
			getpoint(i).dpos = getpoint(i).pos;
			getpoint(i).dvel = getpoint(i).vel;
		}
        prepare_force();
		for(uint i = 0; i < size; i++) {
			eval_derr(t, getpoint(i));
			getpoint(i).pos += getpoint(i).dpos * h;
			getpoint(i).vel += getpoint(i).dvel * h;
			getpoint(i).fo(0, 0);
		}

	}
}


// main integrating method for all points using RK4
// uses step h, until time = endt
// ---------- OK, eewww, ugly, I know...
void CRK4::solve(float h, float endt) {
    uint size = getsize();
	for(; endt > 0; endt -= h) {
		for(uint i = 0; i < size; i++) {
		    store_pos[i] = getpoint(i).pos;
		    store_vel[i] = getpoint(i).vel;
			getpoint(i).dpos = store_pos[i];
			getpoint(i).dvel = store_vel[i];
		}
		prepare_force();
		for(uint i = 0; i < size; i++) {
			eval_derr(t, getpoint(i));
			final_pos[i] = getpoint(i).dpos / 6;
			final_vel[i] = getpoint(i).dvel / 6;
			getpoint(i).dpos *= h / 2;
			getpoint(i).pos = getpoint(i).dpos + store_pos[i];
			getpoint(i).dvel *= h / 2;
			getpoint(i).vel = getpoint(i).dvel + store_vel[i];
			getpoint(i).fo(0, 0);
		}
		prepare_force();
		for(uint i = 0; i < size; i++) {
			eval_derr(t, getpoint(i));
			final_pos[i] += getpoint(i).dpos / 3;
			final_vel[i] += getpoint(i).dvel / 3;
			getpoint(i).dpos *= h / 2;
			getpoint(i).pos = getpoint(i).dpos + store_pos[i];
			getpoint(i).dvel *= h / 2;
			getpoint(i).vel = getpoint(i).dvel + store_vel[i];
			getpoint(i).fo(0, 0);
		}
		prepare_force();
		for(uint i = 0; i < size; i++) {
			eval_derr(t, getpoint(i));
			final_pos[i] += getpoint(i).dpos / 3;
			final_vel[i] += getpoint(i).dvel / 3;
			getpoint(i).dpos *= h;
			getpoint(i).pos = getpoint(i).dpos + store_pos[i];
			getpoint(i).dvel *= h;
			getpoint(i).vel = getpoint(i).dvel + store_vel[i];
			getpoint(i).fo(0, 0);
		}
		prepare_force();
		for(uint i = 0; i < size; i++) {
			eval_derr(t, getpoint(i));
			final_pos[i] += getpoint(i).dpos / 6;
			final_vel[i] += getpoint(i).dvel / 6;
			final_pos[i] *= h;
			final_vel[i] *= h;
			getpoint(i).pos = store_pos[i] + final_pos[i];
			getpoint(i).vel = store_vel[i] + final_vel[i];
			getpoint(i).fo(0, 0);
		}
	}
}
/// --------------- end of methods ---------------

/// ------------------ print ---------------------
void Cabs_integr::printp(const point& pt, bool f) {
	if (!f) {
		printf("pos: (%f, %f), ", pt.pos.x, pt.pos.y);
		printf("dpos: (%f, %f), ", pt.dpos.x, pt.dpos.y);
		printf("vel: (%f, %f), ", pt.vel.x, pt.vel.y);
		printf("F: (%f, %f), ", pt.fo.x, pt.fo.y);
		printf("mass: (%f)\n", pt.mass);
	}
	else {
		FILE* out = freopen("wire_pts_test.txt", "a", stdout);
		printf("pos: (%f, %f), ", pt.pos.x, pt.pos.y);
		printf("dpos: (%f, %f), ", pt.dpos.x, pt.dpos.y);
		printf("vel: (%f, %f), ", pt.vel.x, pt.vel.y);
		printf("F: (%f, %f), ", pt.fo.x, pt.fo.y);
		printf("mass: (%f)\n", pt.mass);
		fclose (out);
	}
}
